{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 车辆运动学模型\n",
    "\n",
    "详细推导见[博客](https://blog.csdn.net/weixin_42301220/article/details/124747072?spm=1001.2014.3001.5501)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import math"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "###  1. 以车辆重心为中心的单车运动学模型\n",
    "\n",
    "![在这里插入图片描述](https://img-blog.csdnimg.cn/5758bfcc51dd434db2a31a192392957a.png#pic_center)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![在这里插入图片描述](https://img-blog.csdnimg.cn/0183068c18c24cd99a6eaecde18fa05e.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class KinematicModel_1:\n",
    "  \"\"\"假设控制量为前后轮的转向角delta_f，delta_r和加速度a\n",
    "  \"\"\"\n",
    "\n",
    "  def __init__(self, x, y, psi, v, l_r, l_f, dt):\n",
    "    self.x = x\n",
    "    self.y = y\n",
    "    self.psi = psi\n",
    "    self.v = v\n",
    "    self.l_f = l_f\n",
    "    self.l_r = l_r\n",
    "    # 实现是离散的模型\n",
    "    self.dt = dt\n",
    "\n",
    "  def update_state(self, a, delta_f,delta_r):\n",
    "    beta = math.atan((self.l_r*math.tan(delta_f)+self.l_f*math.tan(delta_r))/(self.l_f+self.l_r))\n",
    "    self.x = self.x+self.v*math.cos(self.psi+beta)*self.dt\n",
    "    self.y = self.y+self.v*math.sin(self.psi+beta)*self.dt\n",
    "    self.psi = self.psi+self.v*math.cos(beta)*(math.tan(delta_f)-math.tan(delta_r))/(self.l_f+self.l_r)*self.dt\n",
    "    self.v = self.v+a*self.dt\n",
    "\n",
    "  def get_state(self):\n",
    "    return self.x, self.y, self.psi, self.v\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2. 以前轮驱动的单车运动学模型\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![在这里插入图片描述](https://img-blog.csdnimg.cn/85ca4affb4984b34995367f6e400ea0a.png#pic_center)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![在这里插入图片描述](https://img-blog.csdnimg.cn/50df4dc9b1f2483cb2ec341996f79242.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "class KinematicModel_2:\n",
    "  \"\"\"假设控制量为前轮的转向角delta_f和加速度a\n",
    "  \"\"\"\n",
    "  def __init__(self, x, y, psi,v,l_r,l_f,dt):\n",
    "    self.x = x\n",
    "    self.y = y\n",
    "    self.psi = psi\n",
    "    self.v = v\n",
    "    self.l_f = l_f\n",
    "    self.l_r = l_r\n",
    "    # 实现是离散的模型\n",
    "    self.dt=dt\n",
    "  \n",
    "\n",
    "  def update_state(self,a,delta_f):\n",
    "    beta = math.atan((self.l_r)/(self.l_f+self.l_r)*math.tan(delta_f))\n",
    "    self.x = self.x+self.v*math.cos(self.psi+beta)*self.dt\n",
    "    self.y = self.y+self.v*math.sin(self.psi+beta)*self.dt\n",
    "    self.psi = self.psi+self.v*math.sin(beta)/self.l_r*self.dt\n",
    "    self.v = self.v+a*self.dt\n",
    "\n",
    "  def get_state(self):\n",
    "    return self.x, self.y, self.psi, self.v\n",
    "      "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3. 以后轴中心为车辆中心的单车运动学模型"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![在这里插入图片描述](https://img-blog.csdnimg.cn/295bfb9ee6944e00b68c374a9ae0e9e1.png#pic_center)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![在这里插入图片描述](https://img-blog.csdnimg.cn/98de36e913bd4fcd86b4f3ac933b0afc.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "class KinematicModel_3:\n",
    "  \"\"\"假设控制量为转向角delta_f和加速度a\n",
    "  \"\"\"\n",
    "  def __init__(self, x, y, psi,v,L,dt):\n",
    "    self.x = x\n",
    "    self.y = y\n",
    "    self.psi = psi\n",
    "    self.v = v\n",
    "    self.L = L\n",
    "    # 实现是离散的模型\n",
    "    self.dt=dt\n",
    "  \n",
    "\n",
    "  def update_state(self,a,delta_f):\n",
    "    self.x = self.x+self.v*math.cos(self.psi)*self.dt\n",
    "    self.y = self.y+self.v*math.sin(self.psi)*self.dt\n",
    "    self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt\n",
    "    self.v = self.v+a*self.dt\n",
    "\n",
    "  def get_state(self):\n",
    "    return self.x, self.y, self.psi, self.v\n",
    "      "
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "0c7484b3574347463e16b31029466871583b0d4e5c4ad861e8848f2d3746b4de"
  },
  "kernelspec": {
   "display_name": "Python 3.8.12 ('gobigger')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.12"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
